//
// Created by codex on 2024/7/15.
//
#include "math.h"
#include "calc.h"
#include "Drv_UpperComputer.h"

extern _upper_position_st up_pos;
extern _upper_cmd_st up_cmd;
//void chatterCallback(const geometry_msgs::PoseStamped& msg)
//{
//q0=msg.pose.orientation.w;
//q1=msg.pose.orientation.x;
//q2=msg.pose.orientation.y;
//q3=msg.pose.orientation.z;
//qx=msg.pose.position.x * 100;
//qy=msg.pose.position.y * 100;
//ros_pitch = (float)(asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3); // pitch
//ros_roll = (float)(atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3); // roll
//ros_yaw = (float)(atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3); //yaw
//}

void Get_Eulerangle(float *Ptich, float *Roll, float *Yaw)
{
//    *Ptich=ros_pitch;
//    *Roll=ros_roll;
        *Yaw=up_pos.yaw;
}
void Get_now_xy(float *x, float *y)
{
    *x=(float)up_pos.upper_pos_x*100.0;
    *y=(float)up_pos.upper_pos_y*100.0;
}
float Get_Euclidean_Distance(float x, float y)
{
    return (sqrtf(pow(x, 2) + pow(y, 2)));
}

float Get_angle(float x1, float y1, float x2, float y2)
{//TODO:debug
    float xx = x2 - x1;
    float yy = y2 - y1;
    float angle;
//    print_UARTx(&huart1, "(%d %d)\r\n", (int)xx, (int)yy);

    angle = atan2(yy, xx) * _VAL;

//    angle -= 90;
    if(xx < 0 && yy < 0)
    {
        angle = angle + 270.0;
    }
    else
    {
        angle = angle - 90.0;
    }

    if(xx < 10 && xx > -10)
        if(yy < 0)
            angle = 180;
        else
            angle = 0;
    return angle;
}
